/* 
 * File: iRobotSensors.c
 * Author: Tetsuya Idota and Brian Fehrman
 * Description: Contains function implementations for reading robot Sensors
 * Created on (date): 2-5-2013
 */

#include "iRobotSensors.h"


////////////////////
// Function : display_sensor_readings
// Authors: Tetsuya Idota and Brian Fehrman
//
// Purpose: Retrieves sensor readings from robot
//			and transmits those readings to
//			another device via USART0.
/////////////////////
void display_sensor_readings( )
{
	int16_t angle = get_angle( );
	uint16_t battery_capacity = get_battery_capacity( );
	int8_t battery_temp = get_battery_temp( );
	uint8_t bumper_and_wheel_states = get_bumper_and_wheel_states( );
	uint8_t bumper_states = bumper_and_wheel_states & 0x03;
	uint8_t charging_state = get_charging_state();
	uint8_t cliff_FL = get_cliff_FL( );
	uint8_t cliff_FR = get_cliff_FR( );
	uint8_t cliff_L = get_cliff_L( );
	uint8_t cliff_R = get_cliff_R( );
	int8_t current = get_current( );
	int16_t distance = get_distance( );
	uint8_t OI_mode = get_OI_mode( );
	uint16_t voltage = get_voltage( );
	
	//Determine charging state and print appropriate message
	printf( "Charging state: " );
	switch( charging_state )
	{
		case 1:
			printf( "Reconditioning Charging \n" );
			break;
			
		case 2:
			printf( "Full Charging \n" );
			break;
			
		case 3:
			printf( "Trickle Charging \n" );
			break;
			
		case 4:
			printf( "Waiting \n" );
			break;
			
		case 5:
			printf( "Charge Fault Condition \n" );
			break;
			
		default:
			printf( "Not Charging \n" );
		
	}
	
	printf( "Voltage: %u mV \n", voltage );
	printf( "Current: %d mV \n", current );
	printf( "Battery Temp: %d \n", battery_temp);
	printf( "Battery Capacity: %u \n", battery_capacity ); 
	
	//Determine bumper state and print appropriate message
	printf( "Bumper state: " );
	switch( bumper_states )
	{
		case 1:
			printf( "Right Bumper Pressed \n" );
			break;
			
		case 2:
			printf( "Left Bumper Pressed \n" );
			break;
			
		case 3:
			printf( "Both Bumpers Pressed \n" );
			break;
			
		default:
			printf( "No Bumpers Pressed \n" );
	}
	
	//Can probably condense the 4 values into a single "cliff" struct later
	printf( "Cliff FL (0 = No Cliff, 1 = Cliff) : %u \n", cliff_FL );
	printf( "Cliff FR (0 = No Cliff, 1 = Cliff) : %u \n", cliff_FR );
	printf( "Cliff L (0 = No Cliff, 1 = Cliff) : %u \n", cliff_L );
	printf( "Cliff R (0 = No Cliff, 1 = Cliff) : %u \n", cliff_R );
	
	//Determine OI mode and print appropriate message
	printf( "OI Mode: ");
	switch( OI_mode )
	{
		case 1:
			printf( "Passive \n" );
			break;
			
		case 2:
			printf( "Safe \n" );
			break;
			
		case 3:
			printf( "Full \n" );
			break;
			
		default:
			printf( "Off \n" );
	}
	
	printf( "Distance traveled since last read: %d mm \n", distance );
	printf( "Angle traveled since last read: %d mm \n", angle );
}

///////////////////////
// Function: get_angle
// Authors: Tetsuya Idota and Brian Fehrman
//
// Purpose: Gets angle traveled since last reading
//
//		-32768 - 32767 mm
///////////////////////
int16_t get_angle( )
{
	int16_t angle= 0;
	
	usart1tx( ( uint8_t ) 142 );
	_delay_ms( 20 );
	usart1tx( ( uint8_t ) 19 );
	_delay_ms( 20 );
	
	angle  = usart1rx();
	angle <<= 8;
	angle  |= ( int16_t ) usart1rx();
	
	return angle;
}


///////////////////////
// Function: get_battery_capacity
// Authors: Tetsuya Idota and Brian Fehrman
//
// Purpose: Gets the capacity of the battery
//
//		0 - 65535 mAh
///////////////////////
uint16_t get_battery_capacity( )
{
	uint16_t capacity = 0;
	
	usart1tx( ( uint8_t ) 142 );
	_delay_ms( 20 );
	usart1tx( ( uint8_t ) 26 );
	_delay_ms( 20 );
	
	capacity  = usart1rx();
	capacity <<= 8;
	capacity  |= ( uint16_t ) usart1rx();
	
	return capacity;
}

///////////////////////
// Function: get_battery_temp
// Authors: Tetsuya Idota and Brian Fehrman
//
// Purpose: Gets temperature of the battery
//
//		-128 - 127 Degrees Celsius
///////////////////////
int8_t get_battery_temp( )
{
	usart1tx( ( uint8_t ) 142 );
	_delay_ms( 20 );
	usart1tx( ( uint8_t ) 24 );
	_delay_ms( 20 );
	
	return usart1rx();
}

///////////////////////
// Function: get_bumper_states
// Authors: Tetsuya Idota and Brian Fehrman
//
// Purpose: Gets the state of the bumpers and wheels
//
//	(The following refers to which bits are set in the byte
//		as opposed to the actual value of the byte)
//
//		0 : Right bumper pressed
//		1 : Left bumper pressed
//		2 : Wheeldrop Right
//		3 : Wheeldrop Left
//		4 : Wheeldrop Caster
///////////////////////
uint8_t get_bumper_and_wheel_states( )
{
	usart1tx( ( uint8_t ) 142 );
	_delay_ms( 20 );
	usart1tx( ( uint8_t ) 7 );
	_delay_ms( 20 );
	
	return usart1rx();
}

/////////////////////
// Function : get_charging_state
// Authors: Tetsuya Idota and Brian Fehrman
//
// Purpose: Retrieves the charging state of the robot
//
//		0 : Not charging
//		1 : Reconditioning Charging
//		2 : Full Charging
//		3 : Trickle Charging
//		4 : Waiting
//		5 : Charging Fault Condition
/////////////////////
uint8_t get_charging_state( )
{
	usart1tx( ( uint8_t ) 142 );
	_delay_ms( 20 );
	usart1tx( ( uint8_t ) 21 );
	_delay_ms( 20 );
	
	return usart1rx();
}

///////////////////////
// Function: get_cliff_FL
// Authors: Tetsuya Idota and Brian Fehrman
//
// Purpose: Gets the state of the front left cliff sensor
//
//		0 or 1
///////////////////////
uint8_t get_cliff_FL( )
{
	usart1tx( ( uint8_t ) 142 );
	_delay_ms( 20 );
	usart1tx( ( uint8_t ) 10 );
	_delay_ms( 20 );
	
	return usart1rx();
}

///////////////////////
// Function: get_cliff_FR
// Authors: Tetsuya Idota and Brian Fehrman
//
// Purpose: Gets the state of the front right cliff sensor
//
//		0 or 1
///////////////////////
uint8_t get_cliff_FR( )
{
	usart1tx( ( uint8_t ) 142 );
	_delay_ms( 20 );
	usart1tx( ( uint8_t ) 11 );
	_delay_ms( 20 );
	
	return usart1rx();
}

///////////////////////
// Function: get_cliff_L
// Authors: Tetsuya Idota and Brian Fehrman
//
// Purpose: Gets the state of the left cliff sensor
//
//		0 or 1
///////////////////////
uint8_t get_cliff_L( )
{
	usart1tx( ( uint8_t ) 142 );
	_delay_ms( 20 );
	usart1tx( ( uint8_t ) 9 );
	_delay_ms( 20 );
	
	return usart1rx();
}

///////////////////////
// Function: get_cliff_R
// Authors: Tetsuya Idota and Brian Fehrman
//
// Purpose: Gets the state of the right cliff sensor
//
//		0 or 1
///////////////////////
uint8_t get_cliff_R( )
{
	usart1tx( ( uint8_t ) 142 );
	_delay_ms( 20 );
	usart1tx( ( uint8_t ) 12 );
	_delay_ms( 20 );
	
	return usart1rx();
}

/////////////////////
// Function : get_current
// Authors: Tetsuya Idota and Brian Fehrman
//
// Purpose: Retrieves the current of the robot
//
//		-32768 - 32767 mA
/////////////////////
int16_t get_current( )
{
	int16_t current = 0;
	
	usart1tx( ( uint8_t ) 142 );
	_delay_ms( 20 );
	usart1tx( ( uint8_t ) 23 );
	_delay_ms( 20 );
	
	current  = usart1rx();
	current  <<= 8;
	current  |= ( int16_t ) usart1rx();
	
	return current ;
}

///////////////////////
// Function: get_distance
// Authors: Tetsuya Idota and Brian Fehrman
//
// Purpose: Gets distance traveled since last reading
//
//		-32768 - 32767 mm
///////////////////////
int16_t get_distance( )
{
	int16_t distance= 0;
	
	usart1tx( ( uint8_t ) 142 );
	_delay_ms( 20 );
	usart1tx( ( uint8_t ) 19 );
	_delay_ms( 20 );
	
	distance  = usart1rx();
	distance <<= 8;
	distance  |= ( int16_t ) usart1rx();
	
	return distance;
}

///////////////////////
// Function: get_OI_mode
// Authors: Tetsuya Idota and Brian Fehrman
//
// Purpose: Gets the OI mode of the robot
//
//		0 : off
//		1 : Passive
//		2 : Safe
//		3 : Full
///////////////////////
uint8_t get_OI_mode( )
{
	usart1tx( ( uint8_t ) 142 );
	_delay_ms( 20 );
	usart1tx( ( uint8_t ) 35 );
	_delay_ms( 20 );
	
	return usart1rx();
}

/////////////////////
// Function : voltage
// Authors: Tetsuya Idota and Brian Fehrman
//
// Purpose: Retrieves the voltage of the robot
//
//		0 - 65535 mV
/////////////////////
uint16_t get_voltage( )
{
	uint16_t voltage = 0;
	
	usart1tx( ( uint8_t ) 142 );
	_delay_ms( 20 );
	usart1tx( ( uint8_t ) 22);
	_delay_ms( 20 );
	
	voltage = usart1rx();
	voltage <<= 8;
	voltage |= ( uint16_t ) usart1rx();
	
	return voltage;
}
